using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;

namespace Aima.Core.Util.datastructure
{
    public class PriorityQueue
    {
        private readonly SortedDictionary<int, Queue> _storage;
        private int _totalSize;

        public PriorityQueue()
        {
            _storage = new SortedDictionary<int, Queue>();
            _totalSize = 0;
        }

        public bool IsEmpty()
        {
            return (_totalSize == 0);
        }

        public object Dequeue()
        {
            if (IsEmpty())
            {
                throw new Exception("Please check that priorityQueue is not empty before dequeing");
            }
            
            foreach (Queue q in _storage.Values)
            {
                // use a sorted dictionary                     
                if (q.Count > 0)
                {
                    _totalSize--;
                    return q.Dequeue();
                }
            }
            Debug.Assert(false, "not supposed to reach here. problem with changing total_size");
            return null; // not supposed to reach here.         
        }

        // same as above, except for peek.          
        public object Peek()
        {
            if (IsEmpty())
                throw new Exception("Please check that priorityQueue is not empty before dequeing");

            foreach (Queue q in _storage.Values)
            {
                if (q.Count > 0)
                    return q.Peek();
            }
            Debug.Assert(false, "not supposed to reach here. problem with changing total_size");
            return null; // not supposed to reach here.         
        }

        public object Dequeue(int prio)
        {
            _totalSize--;
            return _storage[prio].Dequeue();
        }

        public void Enqueue(object item, int prio)
        {
            if (!_storage.ContainsKey(prio))
            {
                _storage.Add(prio, new Queue());
                Enqueue(item, prio);
                // run again              
            }
            else
            {
                _storage[prio].Enqueue(item);
                _totalSize++;
            }
        }
    }
}

/**
 * Artificial Intelligence A Modern Approach (3rd Edition): pg 80.<br>
 * <br>
 * The priority queue, which pops the element of the queue with the highest
 * priority according to some ordering function.
 * 
 * @author Ciaran O'Reilly
 */